#ifndef ZHUAGOU_HPP
#define ZHUAGOU_HPP
#include <string>
#include <vector>
#include <iostream>
#include <jsoncpp/json/json.h>
class ZhuaGou
{
    public :
     ZhuaGou()
     {
       init_status();
       err_code=0;
        updis=0.0;
        downdis=0.0;
        set_seat_downdis=0;
     }
     ~ZhuaGou(){}
     void init_status(void)
     {
         zhua_status=0;
        zhua_cur_cmd=0;
        zhua_step=0;
        zhua_old_cmd=0;
        time_count=0;
        time_end=0;
        cur_act=0;
      
     }
 ///value define
    int zhua_status;
    int zhua_cur_cmd;
    int zhua_step;    
    int zhua_old_cmd;
    int time_count,time_end;
    int cur_act;
    float updis,downdis;
    float still_downdis;
    float set_seat_downdis;
    int err_code;
    std::string status_msg;
    std::string cmd_msg;
    int zhua_time_process(void);
    void zhua_down0(void);
    void zhua_up0(void);
    void zhua_down1(void);
    void zhua_up1(void);
    std::string  get_zhua_status_msg(void);
    void set_cmd_msg(std::string msg);
    void zhuagou_in(void);
    void zhuagou_out(void);
    void zhuagou_updown_to_ground(void);
};
void ZhuaGou::zhuagou_updown_to_ground(void)
{
  time_count++;
    switch(zhua_step)
    {
      case 0:
        time_count=0;
        zhua_step++;
        cur_act=0;
        if(set_seat_downdis<0.02) zhua_step=4;
      break;
      case 1:
        if(set_seat_downdis<downdis)zhua_step=2;
         else zhua_step=3;
      break;
      case 2:
        cur_act=8;
        if(set_seat_downdis>downdis)zhua_step=4;
      break;
      case 3:
        cur_act=4;
        if(set_seat_downdis<downdis)zhua_step=4;
      break;  
      case 4:
        cur_act=0;
        init_status();
      break;
    }
}
void ZhuaGou::zhuagou_in(void)
{
  time_count++;
    switch(zhua_step)
    {
      case 0:
        time_count=0;
        zhua_step++;
        cur_act=0;
      break;
      case 1:
        cur_act=2;
       if(time_count==200)
        {
           time_count=0;
           zhua_step++;
        }
      break;
      case 2:
        cur_act=0;
        init_status();
      break;
    }
  //  return cur_act;
}
void ZhuaGou::zhuagou_out(void)
{
  time_count++;
    switch(zhua_step)
    {
      case 0:
        time_count=0;
        zhua_step++;
        cur_act=0;
      break;
      case 1:
        cur_act=1;
       if(time_count==200)
        {
           time_count=0;
           zhua_step++;
        }
      break;
      case 2:
        cur_act=0;
        init_status();
      break;
    }
   // return cur_act;
}
void ZhuaGou::set_cmd_msg(std::string msg)
{
   Json::Reader reader;
   Json::Value json_val;
   
   if(reader.parse(msg, json_val))
     {
      zhua_cur_cmd=json_val["cmd_value"].asInt();
      // std::cout<<"cmd ="<<zhua_cur_cmd<<std::endl;
      // std::cout<<std::flush;
      if(json_val.isMember("set_seat_downdis"))
      {
        set_seat_downdis=json_val["set_seat_downdis"].asFloat();
        // std::cout<<"read set_seat_downdis"<<set_seat_downdis<<std::endl;
        // std::cout<<std::flush;
      }
     }
}
std::string  ZhuaGou::get_zhua_status_msg(void)
{
  Json::Value json_val;
  json_val["zhua_cur_cmd"] =zhua_cur_cmd;
  json_val["zhua_step"] =zhua_step;
  json_val["cur_act"] =cur_act;
  status_msg = json_val.toStyledString();
  return status_msg;
}

int ZhuaGou::zhua_time_process(void)
{
    // if((zhua_cur_cmd==0)&&(cmd==0))return 0;
    // if((zhua_cur_cmd==0)&&(cmd!=0))
    // {
    //    init_status();
    //    zhua_cur_cmd=cmd; 
    // }
    // if(cmd==0) init_status();   //执行完命令初始化
   
    switch(zhua_cur_cmd)
    {
      case 0:
        
        cur_act=0;
      break;
      case 1:
        zhua_down0();  //放下并收回抓钩
      break;
      case 2:
        zhua_up0();  ////抓取大罐。
      break;
      case 3:
        zhua_down1();  //放下并收回抓钩
      break;
      case 4:
        zhua_up1();  ////抓取小罐。
      break;
      case 10:
        zhuagou_updown_to_ground();  //设定地面距离升降抓钩
      break;
      case 12:
        zhuagou_in();
      break;
      case 11:
        zhuagou_out();
        break;
      // if(zhua_cur_cmd!=0) //执行有效命令
      // {
      //   if(zhua_cur_cmd==1)  //放下并收回抓钩
      //     zhua_down0();  
      //   if(zhua_cur_cmd==2) //抓取大罐。 
      //     zhua_up0();
      // }
    }
    return cur_act;
}
void ZhuaGou::zhua_down0(void)
{
    time_count++;
   switch(zhua_step)
  {
    case 0:
      cur_act=0;
      if(downdis<0.07)  //说明还挂着东西
        zhua_step++;
      else 
        zhua_step=6;
    break;
    case 1:
      cur_act=0;
      if(downdis>0.04)  //说明还挂着东西
        zhua_step++;
      else 
        zhua_step=6;
      still_downdis=downdis;  //将当前抓钩和物体的距离记录下来
      time_count=0;
     break;
     case 2:
      cur_act=8;
      if((still_downdis-0.005)>downdis)
      {
        time_count=0;
        cur_act=0;
        zhua_step++;
      }
     break;
      case 3:
       cur_act=0;
       if(time_count==40)
        {
           time_count=0;
           zhua_step++;
        }
      break;
      case 4:
        cur_act=2;
       if(time_count==200)
        {
           time_count=0;
           zhua_step++;
        }
        break;
      case 5:
        cur_act=4;
        ///两个条件用一个
        if(downdis>0.15)
          {
           time_count=0;
           zhua_step++;
        }
      //  if(time_count==400)  ////
      //   {
      //      time_count=0;
      //      zhua_step++;
      //   }
      break;  
      case 6:
      cur_act=0;
      init_status();
      break;  
  }

}
void ZhuaGou::zhua_up0(void)
{
     time_count++;
    switch (zhua_step)
    {
     case 0:
        if(downdis<0.07)  ///err 距离小于，说明还挂着东西
       {
        zhua_step=5;
        time_count=0;
        std::cout<<"downdis="<<downdis<<std::endl;
        std::cout.flush(); 
       }
       else 
        zhua_step =1; 
     break;

     case 1:
        cur_act=8;    //抓钩向下
        if(downdis<0.045)
        {
         zhua_step=2;
         time_count=0;   
         std::cout<<"this="<<std::endl;
        }
        break;
    case 2:
       cur_act=1; 
       if(time_count==200) //按50ms计时 10s
       {
        cur_act=0;
        time_count=0;
        zhua_step=4;
       }
    break;
    case 4:
        cur_act=4; 
        if(time_count==400) //按50ms计时 20s
       {
        cur_act=0;
        time_count=0;
        zhua_step=5;
       }

    break;
    case 5:
        cur_act=0; 
       init_status();
       break;
    }
}
/////////////////////////
////小放射源
void ZhuaGou::zhua_down1(void)
{
    time_count++;
   switch(zhua_step)
  {
    case 0:
      cur_act=0;
      if(downdis<0.07)  //说明还挂着东西
        zhua_step++;
      else 
        zhua_step=6;
    break;
    case 1:
      cur_act=0;
      if(downdis>0.025)  //说明还挂着东西
        zhua_step++;
      else 
        zhua_step=6;
      still_downdis=downdis;  //将当前抓钩和物体的距离记录下来
      time_count=0;
     break;
     case 2:
      cur_act=8;
      if((still_downdis-0.008)>downdis)
      {
        time_count=0;
        cur_act=0;
        zhua_step++;
      }
     break;
      case 3:
       cur_act=0;
       if(time_count==40)
        {
           time_count=0;
           zhua_step++;
        }
      break;
      case 4:
        cur_act=2;
       if(time_count==200)
        {
           time_count=0;
           zhua_step++;
        }
        break;
      case 5:
        cur_act=4;
        ///两个条件用一个
        if(downdis>0.15)
          {
           time_count=0;
           zhua_step++;
        }
      //  if(time_count==400)  ////
      //   {
      //      time_count=0;
      //      zhua_step++;
      //   }
      break;  
      case 6:
      cur_act=0;
      init_status();
      break;  
  }

}
void ZhuaGou::zhua_up1(void)
{
     time_count++;
    switch (zhua_step)
    {
     case 0:
        if(downdis<0.07)  ///err 距离小于，说明还挂着东西
       {
        zhua_step=5;
        time_count=0;
        std::cout<<"downdis="<<downdis<<std::endl;
        std::cout.flush(); 
       }
       else 
        zhua_step =1; 
     break;

     case 1:
        cur_act=8;    //抓钩向下
        if(downdis<0.031)
        {
         zhua_step=2;
         time_count=0;   
         std::cout<<"this="<<std::endl;
        }
        break;
    case 2:
       cur_act=1; 
       if(time_count==200) //按50ms计时 10s
       {
        cur_act=0;
        time_count=0;
        zhua_step=4;
       }
    break;
    case 4:
        cur_act=4; 
        if(time_count==400) //按50ms计时 20s
       {
        cur_act=0;
        time_count=0;
        zhua_step=5;
       }

    break;
    case 5:
        cur_act=0; 
       init_status();
       break;
    }
}
#endif